﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace M3G {
    public class Transform : IEquatable<Transform> {
        #region
        float[] matrix;
        #endregion

        #region Method
        public Transform () {
            matrix = new float[]{1,0,0,0,
                                 0,1,0,0,
                                 0,0,1,0,
                                 0,0,0,1};
        }

        public Transform (float[] matrix)
            : this () {
            if (matrix == null) {
                throw new ArgumentNullException ("Matrix is null");
            }
            if (matrix.Length < 16) {
                throw new ArgumentException ("Length of Matrix is less than required, len=" + matrix.Length + ",req=16");
            }

            Array.Copy (matrix, this.matrix, 16);
        }

        public Transform (Transform transform)
            : this () {

            Array.Copy (transform.matrix, this.matrix, 16);
        }


        public float[] Matrix {
            get {
                return (float[])matrix.Clone ();
            }
            set {
                var matrix = value;
                if (matrix == null) {
                    throw new ArgumentNullException ("Matrix is null");
                }
                if (matrix.Length < 16) {
                    throw new ArgumentException ("Length of Matrix is less than required, len=" + matrix.Length + ",req=16");
                }

                Array.Copy (matrix, this.matrix, 16);
            }
        }

        public void Add (Transform transform) {
            if (transform == null) {
                throw new ArgumentNullException ("Transform is null");
            }

            for (var i = 0; i < 16; i++) {
                this.matrix[i] += transform.matrix[i];
            }
        }

        public float Determinant {
            get {
                var m = matrix;
                var d = m[0] * m[5] * m[10] * m[15] + m[0] * m[6] * m[11] * m[13] + m[0] * m[7] * m[9] * m[14]
                      + m[1] * m[4] * m[11] * m[14] + m[1] * m[6] * m[8] * m[15] + m[1] * m[7] * m[10] * m[12]
                      + m[2] * m[4] * m[9] * m[15] + m[2] * m[5] * m[11] * m[12] + m[2] * m[7] * m[8] * m[13]
                      + m[3] * m[4] * m[10] * m[13] + m[3] * m[5] * m[8] * m[14] + m[3] * m[6] * m[9] * m[12]
                      - m[0] * m[5] * m[11] * m[14] - m[0] * m[6] * m[9] * m[15] - m[0] * m[7] * m[10] * m[13]
                      - m[1] * m[4] * m[10] * m[15] - m[1] * m[6] * m[11] * m[12] - m[1] * m[7] * m[8] * m[14]
                      - m[2] * m[4] * m[11] * m[13] - m[2] * m[5] * m[8] * m[15] - m[2] * m[7] * m[9] * m[12]
                      - m[3] * m[4] * m[9] * m[14] - m[3] * m[5] * m[10] * m[12] - m[3] * m[6] * m[8] * m[13];
                return d;
            }
        }

        public float Determinant3x3 {
            get {
                var m = matrix;
                var d = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] + m[1] * m[6] * m[8]
                      - m[1] * m[4] * m[10] + m[2] * m[4] * m[9] - m[2] * m[5] * m[8];
                return d;
            }
        }
        

        public void Set (Transform transform) {
            if (transform == null) {
                throw new ArgumentNullException ("Transform is null");
            }

            Array.Copy (transform.matrix, this.matrix, 16);
        }

        public void SetIdentity () {
            for (var i=0; i < 16; i++) {
                this.matrix[i] = (i % 5 == 0) ? 1 : 0;
            }
        }



        public void Invert () {
            var d = Determinant;
            if (d == 0) {
                throw new ArithmeticException ("This Transform can't be inverted");
            }
            var m = (float[])matrix.Clone();
            this.matrix[0] = 1 / d * (m[5] * m[10] * m[15] + m[6] * m[11] * m[13] + m[7] * m[9] * m[14] - m[5] * m[11] * m[14] - m[6] * m[9] * m[15] - m[7] * m[10] * m[13]);
            this.matrix[1] = 1 / d * (m[1] * m[11] * m[14] + m[2] * m[9] * m[15] + m[3] * m[10] * m[13] - m[1] * m[10] * m[15] - m[2] * m[11] * m[13] - m[3] * m[9] * m[14]);
            this.matrix[2] = 1 / d * (m[1] * m[6] * m[15] + m[2] * m[7] * m[13] + m[3] * m[5] * m[14] - m[1] * m[7] * m[14] - m[2] * m[5] * m[15] - m[3] * m[6] * m[13]);
            this.matrix[3] = 1 / d * (m[1] * m[7] * m[10] + m[2] * m[5] * m[11] + m[3] * m[6] * m[9] - m[1] * m[6] * m[11] - m[2] * m[7] * m[9] - m[3] * m[5] * m[10]);
            this.matrix[4] = 1 / d * (m[4] * m[11] * m[14] + m[6] * m[8] * m[15] + m[7] * m[10] * m[12] - m[4] * m[10] * m[15] - m[6] * m[11] * m[12] - m[7] * m[8] * m[14]);
            this.matrix[5] = 1 / d * (m[0] * m[10] * m[15] + m[2] * m[11] * m[12] + m[3] * m[8] * m[14] - m[0] * m[11] * m[14] - m[2] * m[8] * m[15] - m[3] * m[10] * m[12]);
            this.matrix[6] = 1 / d * (m[0] * m[7] * m[14] + m[2] * m[4] * m[15] + m[3] * m[6] * m[12] - m[0] * m[6] * m[15] - m[2] * m[7] * m[12] - m[3] * m[4] * m[14]);
            this.matrix[7] = 1 / d * (m[0] * m[6] * m[11] + m[2] * m[7] * m[8] + m[3] * m[4] * m[10] - m[0] * m[7] * m[10] - m[2] * m[4] * m[11] - m[3] * m[6] * m[8]);
            this.matrix[8] = 1 / d * (m[4] * m[9] * m[15] + m[5] * m[11] * m[12] + m[7] * m[8] * m[13] - m[4] * m[11] * m[13] - m[5] * m[8] * m[15] - m[7] * m[9] * m[12]);
            this.matrix[9] = 1 / d * (m[0] * m[11] * m[13] + m[1] * m[8] * m[15] + m[3] * m[9] * m[12] - m[0] * m[9] * m[15] - m[1] * m[11] * m[12] - m[3] * m[8] * m[13]);
            this.matrix[10] = 1 / d * (m[0] * m[5] * m[15] + m[1] * m[7] * m[12] + m[3] * m[4] * m[13] - m[0] * m[7] * m[13] - m[1] * m[4] * m[15] - m[3] * m[5] * m[12]);
            this.matrix[11] = 1 / d * (m[0] * m[7] * m[9] + m[1] * m[4] * m[11] + m[3] * m[5] * m[8] - m[0] * m[5] * m[11] - m[1] * m[7] * m[8] - m[3] * m[4] * m[9]);
            this.matrix[12] = 1 / d * (m[4] * m[10] * m[13] + m[5] * m[8] * m[14] + m[6] * m[9] * m[12] - m[4] * m[9] * m[14] - m[5] * m[10] * m[12] - m[6] * m[8] * m[13]);
            this.matrix[13] = 1 / d * (m[0] * m[9] * m[14] + m[1] * m[10] * m[12] + m[2] * m[8] * m[13] - m[0] * m[10] * m[13] - m[1] * m[8] * m[14] - m[2] * m[9] * m[12]);
            this.matrix[14] = 1 / d * (m[0] * m[6] * m[13] + m[1] * m[4] * m[14] + m[2] * m[5] * m[12] - m[0] * m[5] * m[14] - m[1] * m[6] * m[12] - m[2] * m[4] * m[13]);
            this.matrix[15] = 1 / d * (m[0] * m[5] * m[10] + m[1] * m[6] * m[8] + m[2] * m[4] * m[9] - m[0] * m[6] * m[9] - m[1] * m[4] * m[10] - m[2] * m[5] * m[8]);
        }

        public void Transpose () {
            var m = matrix;
            this.matrix = new float[]{m[0],m[4],m[8],m[12],
                                      m[1],m[5],m[9],m[13],
                                      m[2],m[6],m[10],m[14],
                                      m[3],m[7],m[11],m[15]};
        }


        public void Multiply (float scalar) {
            for (var i=0; i < 16; i++) {
                this.matrix[i] *= scalar;
            }
        }

        // M'=MT
        void PostMultiply (float[] t) {
            var m = (float[])matrix.Clone ();

            this.matrix[0] = m[0] * t[0] + m[1] * t[4] + m[2] * t[8] + m[3] * t[12];
            this.matrix[1] = m[0] * t[1] + m[1] * t[5] + m[2] * t[9] + m[3] * t[13];
            this.matrix[2] = m[0] * t[2] + m[1] * t[6] + m[2] * t[10] + m[3] * t[14];
            this.matrix[3] = m[0] * t[3] + m[1] * t[7] + m[2] * t[11] + m[3] * t[15];

            this.matrix[4] = m[4] * t[0] + m[5] * t[4] + m[6] * t[8] + m[7] * t[12];
            this.matrix[5] = m[4] * t[1] + m[5] * t[5] + m[6] * t[9] + m[7] * t[13];
            this.matrix[6] = m[4] * t[2] + m[5] * t[6] + m[6] * t[10] + m[7] * t[14];
            this.matrix[7] = m[4] * t[3] + m[5] * t[7] + m[6] * t[11] + m[7] * t[15];

            this.matrix[8] = m[8] * t[0] + m[9] * t[4] + m[10] * t[8] + m[11] * t[12];
            this.matrix[9] = m[8] * t[1] + m[9] * t[5] + m[10] * t[9] + m[11] * t[13];
            this.matrix[10] = m[8] * t[2] + m[9] * t[6] + m[10] * t[10] + m[11] * t[14];
            this.matrix[11] = m[8] * t[3] + m[9] * t[7] + m[10] * t[11] + m[11] * t[15];

            this.matrix[12] = m[12] * t[0] + m[13] * t[4] + m[14] * t[8] + m[15] * t[12];
            this.matrix[13] = m[12] * t[1] + m[13] * t[5] + m[14] * t[9] + m[15] * t[13];
            this.matrix[14] = m[12] * t[2] + m[13] * t[6] + m[14] * t[10] + m[15] * t[14];
            this.matrix[15] = m[12] * t[3] + m[13] * t[7] + m[14] * t[11] + m[15] * t[15];
        }

        public void PostMultiply (Transform transform) {
            if (transform == null) {
                throw new ArgumentNullException ("Transform is null");
            }

            PostMultiply (transform.matrix);
        }

 
        public void PostRotate (float angle, float ax, float ay, float az) {
            if (angle != 0 && ax == 0 && ay == 0 && az == 0) {
                throw new ArgumentException ("Rotation axis must be non-zero vector");
            }
            if (angle == 0) {
                return;
            }

            angle *= (float)Math.PI / 180.0f;
            var len = (float)Math.Sqrt (ax * ax + ay * ay + az * az);
            ax /= len;
            ay /= len;
            az /= len;

            var c = (float)Math.Cos (angle);
            var s = (float)Math.Sin (angle);

            var m = new float[]{ax*ax*(1-c)+c   , ax*ay*(1-c)-az*s , ax*az*(1-c)+ay*s, 0,
                                ay*ax*(1-c)+az*s, ay*ay*(1-c)+c    , ay*az*(1-c)-ax*s, 0,
                                ax*az*(1-c)-ay*s, ay*az*(1-c)+ax*s , az*az*(1-c)+c   , 0,
                                0               , 0                , 0               , 1};
            PostMultiply (m);
        }


        public void PostRotateQuat (float qx, float qy, float qz, float qw) {
            if (qx == 0 && qy == 0 && qz == 0 && qw == 0) {
                throw new ArgumentException ("Quaternion must be non-zero vector");
            }

            var len = (float)Math.Sqrt (qx * qx + qy * qy + qz * qz + qw * qw);
            qx = qx / len;
            qy = qy / len;
            qz = qz / len;
            qw = qw / len;

            var m = new float[]{1-(2*qy*qy+2*qz*qz), 2*qx*qy-2*qz*qw    , 2*qx*qz+2*qy*qw    , 0,
                                2*qx*qy+2*qz*qw    , 1-(2*qx*qx+2*qz*qz), 2*qy*qz-2*qx*qw    , 0,
                                2*qx*qz-2*qy*qw    , 2*qy*qz+2*qx*qw    , 1-(2*qx*qx+2*qy*qy), 0,
                                0                  , 0                  , 0                  , 1};
            PostMultiply (m);
        }


        public void PostScale (float sx, float sy, float sz) {
            var m = new float[]{sx,  0,  0, 0,
                                0 , sy,  0, 0,
                                0 ,  0, sz, 0,
                                0 ,  0,  0, 1};
            PostMultiply (m);
        }

        public void PostTranslate (float tx, float ty, float tz) {
            var m = new float[]{1,0,0,tx,
                                0,1,0,ty,
                                0,0,1,tz,
                                0,0,0,1};
            PostMultiply (m);
        }



        public void TransformVectors (float[] vectors) {
            if (vectors == null) {
                throw new ArgumentNullException ("Vectors is null");
            }
            if (vectors.Length % 4 != 0) {
                throw new ArgumentException ("Length of Vectors must be a multiple of 4, len="+vectors.Length);
            }

            var v = (float[])vectors.Clone();
            var m = matrix;
            for (var i = 0; i < vectors.Length; i+=4) {
                vectors[i + 0] = m[0] * v[i + 0] + m[1] * v[i + 1] + m[2] * v[i + 2] + m[3] * v[i + 3];
                vectors[i + 1] = m[4] * v[i + 0] + m[5] * v[i + 1] + m[6] * v[i + 2] + m[7] * v[i + 3];
                vectors[i + 2] = m[8] * v[i + 0] + m[9] * v[i + 1] + m[10] * v[i + 2] + m[11] * v[i + 3];
                vectors[i + 3] = m[12] * v[i + 0] + m[13] * v[i + 1] + m[14] * v[i + 2] + m[15] * v[i + 3];
            }

        }


        /**
         * TransformVectorsの3コンポーネントバージョン
         * もしかしてClone()するより入力の4/3倍のメモリを確保して入れなおしたほうが速いか？？？
         * */
        public void TransformVectors3 (float[] vectors, bool w) {
            if (vectors == null) {
                throw new ArgumentNullException ("Vectors is null");
            }
            if (vectors.Length % 3 != 0) {
                throw new ArgumentException ("Length of Vectors must be a multiple of 3, len=" + vectors.Length);
            }
            var inW = w ? 1.0f : 0.0f;

            var v = (float[])vectors.Clone ();
            var m = matrix;
            for (var i = 0; i < vectors.Length; i += 3) {
                vectors[i + 0] = m[0] * v[i + 0] + m[1] * v[i + 1] + m[2] * v[i + 2] + m[3] * inW;
                vectors[i + 1] = m[4] * v[i + 0] + m[5] * v[i + 1] + m[6] * v[i + 2] + m[7] * inW;
                vectors[i + 2] = m[8] * v[i + 0] + m[9] * v[i + 1] + m[10] * v[i + 2] + m[11] * inW;
                if (w) {
                    var outW = m[12] * v[i + 0] + m[13] * v[i + 1] + m[14] * v[i + 2] + m[15] * inW;
                    vectors[i + 0] /= outW;
                    vectors[i + 1] /= outW;
                    vectors[i + 2] /= outW;
                }
            }
        }

        public void TransformVertexArray (VertexArray input, float[] output, Boolean w) {
            if (input == null) {
                throw new ArgumentNullException ("Input is null");
            }
            var vertexCount = input.VertexCount;
            var required    = input.VertexCount * 4;
            if (output.Length < required) {
                throw new ArgumentException ("Length of Output is less than required, len=" + output.Length + ",req=" + required);
            }

            var tmp = new float[4] { 0, 0, 0, w ? 1 : 0 };
            var v   = new List<float> ();
            for (var i = 0; i < vertexCount; i++) {
                input.Get (i, 1, tmp);
                v.AddRange (tmp);
            }

            var m = matrix;
            for (var i = 0; i < required; i+=4) {
                output[i + 0] = m[0] * v[i + 0] + m[1] * v[i + 1] + m[2] * v[i + 2] + m[3] * v[i + 3];
                output[i + 1] = m[4] * v[i + 0] + m[5] * v[i + 1] + m[6] * v[i + 2] + m[7] * v[i + 3];
                output[i + 2] = m[8] * v[i + 0] + m[9] * v[i + 1] + m[10] * v[i + 2] + m[11] * v[i + 3];
                output[i + 3] = m[12] * v[i + 0] + m[13] * v[i + 1] + m[14] * v[i + 2] + m[15] * v[i + 3];
            }
        }


        public override bool Equals (object obj) {
            if (obj == null) {
                return false;
            }
            var other = obj as Transform;
            if ((object)other == null) {
                return false;
            }
            return matrix.SequenceEqual(other.matrix);
        }

        bool IEquatable<Transform>.Equals (Transform o) {
            if (o == null) {
                return false;
            }
            return matrix.SequenceEqual(o.matrix);
        }

        public bool Equals (Transform o) {
            if (o == null) {
                return false;
            }
            return matrix.SequenceEqual(o.matrix);
        }

        public static bool operator == (Transform a, Transform b) {
            if ((object)a == null || (object)b == null) {
                return false;
            }
            return a.matrix.SequenceEqual(b.matrix);
        }

        public static bool operator != (Transform a, Transform b) {
            return !(a == b);
        }


        public override int GetHashCode () {
            return matrix.GetHashCode ();
        }


        #endregion

    }
}
